#ifndef MATH_H
#define MATH_H

#include <cstdio>
#include <cmath>

#include "data_structure.h"

/**
 * 三维极坐标转直角坐标
 * 极坐标：(r, theta, phi) -> 直角坐标：(x, y, z)
 * theta：方位角（xy平面），phi：俯仰角（xz平面）
 */
void polar3d_to_cartesian(const RadarPoint3D *point, double *x, double *y, double *z);

// void polar3d_to_cartesian(const RadarPoint3D *point, double *x, double *y, double *z){




// }


/**
 * 三维直角坐标转极坐标
 * 直角坐标：(x, y, z) -> 极坐标：(r, theta, phi)
 */
void cartesian_to_polar3d(double x, double y, double z, double *r, double *theta, double *phi);

/**
 * 3x3矩阵乘法：C = A * B
 */
void mat33_mult(const double A[3][3], const double B[3][3], double C[3][3]);

/**
 * 6x6矩阵乘法：C = A * B
 */
void mat66_mult(const double A[6][6], const double B[6][6], double C[6][6]);

/**
 * 6x6矩阵转置：B = A^T
 */
void mat66_transpose(const double A[6][6], double B[6][6]);

/**
 * 3x3矩阵求逆（针对对称矩阵，简化计算）
 * 返回值：true=成功，false=失败（行列式接近0）
 */
bool mat33_inv(const double A[3][3], double invA[3][3]);

#endif